/*
%五次多项式的参数计算
% coefficients (a0, a1, a2, a3, a4, a5)
*/

#include "QuinticPolynomial.h"
#include <iostream>

using namespace Eigen;
//a0和a1可通过初始化列表赋值
QuinticPolynomial::QuinticPolynomial(double d0, double dd0, double ddd0, double goal_d0, double goal_dd0, double goal_ddd0, double t) : a0(d0), a1(dd0)
{
    Max_s = t;
    a2 = ddd0 / 2.0;
    Matrix3d A;
    Vector3d B;
    A << pow(t, 3), pow(t, 4), pow(t, 5), 3 * pow(t, 2),
        4 * pow(t, 3), 5 * pow(t, 4), 6 * t, 12 * pow(t, 2),
        20 * pow(t, 3);
    B << goal_d0 - a0 - a1 * t - a2 * pow(t, 2), goal_dd0 - a1 - 2 * a2 * t,
        goal_ddd0 - 2 * a2;
    Matrix3d A_inv = A.inverse();
    Vector3d x = A_inv * B;
    a3 = x[0];
    a4 = x[1];
    a5 = x[2];
}

double QuinticPolynomial::getSmax()
{
    return Max_s;
}

double QuinticPolynomial::quint_point(double t )
{
    return a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4) + a5 * pow(t, 5);
}

double QuinticPolynomial::quint_first_derivative(double t )
{
    return a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) + 5 * a5 * pow(t, 4);
}
double QuinticPolynomial::quint_second_derivative(double t )
{
    return 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
}
double QuinticPolynomial::quint_third_derivative(double t )
{
    return 6 * a3 + 24 * a4 * t + 60 * a5 * pow(t, 2);
}